#include <ros/ros.h>
#include <pcl/common/transforms.h>
#include "glog/logging.h"
#include "lidar_localization/subscriber/cloud_subscriber.hpp"
const std::string WORK_SPACE_PATH = "/home/ros_car/src/lidar_localization";
int main(int argc, char **argv) {
    google::InitGoogleLogging(argv[0]);
    FLAGS_log_dir = WORK_SPACE_PATH + "/Log";
    FLAGS_alsologtostderr = true;

    ros::init(argc, argv, "test_frame_node");
    ros::NodeHandle nh;

    std::shared_ptr<CloudSubscriber> cloud_sub_ptr = std::make_shared<CloudSubscriber>(nh, "/kitti/velo/pointcloud", 100000);
    std::deque<CloudData> cloud_data_buff;
        
    ros::Rate rate(100);
    while (ros::ok()) {
        // 解析接收到的点云数据
        cloud_subscriber.ParseData(cloud_data_buff);

        // 如果有新数据，打印点云数据的时间戳和点云大小
        if (!cloud_data_buff.empty()) {
            for (const auto& cloud_data : cloud_data_buff) {
                ROS_INFO("Received point cloud with timestamp: %.2f and size: %zu", 
                         cloud_data.time, cloud_data.cloud_ptr->points.size());
            }
            cloud_data_buff.clear(); // 清空缓冲区
        }

        // 处理 ROS 回调
        ros::spinOnce();
        rate.sleep();
    }


}